#include "energies.h"
#include <cmath>


namespace kuai {
	
	extern RealNumber  
	        bond_energy_harm_0(
	            RealNumber x[], RealNumber k[], 
	            AtomIndex* items, int number)
	{
		RealNumber e = 0;
		for (const AtomIndex* p = items; p  != items + number; ) {
			AtomIndex a1 = *(p++), a2 = *(p++), ik = *(p++);
			RealNumber rx = x[a1]-x[a2], ry = x[a1+1] - x[a2+1], rz = x[a1+2] - x[a2+2];
			RealNumber r0 = k[ik], e0 = k[ik+1];
			RealNumber dr = sqrt(rx*rx+ry*ry+rz*rz) - r0;
			e += e0 * dr * dr;
		}
		return e ;
	}

	extern RealNumber  
	        bond_energy_harm_1(
	            RealNumber x[], RealNumber k[], 
	            AtomIndex* items, int number, 
	            RealNumber result[])
	{
		RealNumber e = 0;
		for (const AtomIndex* p = items; p  != items + number; ) {
			AtomIndex a1 = *(p++), a2 = *(p++), ik = *(p++);
			RealNumber rx = x[a1]-x[a2], ry = x[a1+1] - x[a2+1], rz = x[a1+2] - x[a2+2];
			RealNumber r0 = k[ik], e0 = k[ik+1];
			RealNumber dr = sqrt(rx*rx+ry*ry+rz*rz);
			RealNumber dedr = 2 * e0 / dr;
			dr -= r0;
			e += e0 * dr * dr;
			rx *= dedr; ry *= dedr; rz *= dedr;
			result[a1] += rx;  result[a1+1] += ry; result[a1+2] += rz;
			result[a2] -= rx;  result[a2+1] -= ry; result[a2+2] -= rz;
		}
		return e ;
	}

}

